#define SHOW_GRAPHICS false

#include "PathCalculator.h"
#include "KinematicModel\Point.h"

//Coordinates systems are different for the kinematic model and the collision world, 
//while the x axis are the same, the kinematic Y axis is the same as the collision world Z axis 
//and the kinematic Z axis is the same as the collision world Y axis with opposite direction
#define STARTX SHOULDERX
#define STARTY -(SHOULDERZ - FIRST_LINK_LENGTH - SECOND_LINK_LENGTH - HAND_HEIGHT - HAND_CENTER_OFFSET)
#define STARTZ SHOULDERY

#define NUMBER_OF_TRAJECTORY_POINTS 20

//=============================
// Displaying cubes is very long!
// so we limit their numbers.
 const int MAX_DISPLAYED_CUBES(15000);
//=============================



dirKinResult dirKin(double teta1, double teta2, double teta3, double teta4);
invKinResult invKin(Point p);
invKinResult invKin(Point p, bool recursive);
bool move(Point start, Point object, CollisionWorldViewer* CWV, vector<Point>* puntiDiVia, bool checkCollisions);
bool lmove(Point start, Point object, CollisionWorldViewer* CWV, vector<Point>* puntiDiVia);
void frontalApproach(Point start, Point object, CollisionWorldViewer* CWV, vector<Point> *path);
void upperApproach(Point start, Point object, CollisionWorldViewer* CWV, vector<Point> *path, double maxY);

 PathCalculator::PathCalculator (std::vector<std::string> cloud_path, double resolution, std::vector< object > objects, int no, int movementClass) :
    cloud (new pcl::PointCloud<pcl::PointXYZ>()), octree (resolution)
  {


	double minY, maxY, minX, maxX;
	Point avg(0,0,0);
	int i;
	Point centroid, p, start;

	displayedDepth = static_cast<int> (floor ((float) octree.getTreeDepth()));
    if (displayedDepth == 0)
      displayedDepth = 1;

    //.pcd file of the object to grasp loading
	if (!loadCloud(cloud_path.at(no)))
      return;


	std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ> > centers;
	octree.getOccupiedVoxelCenters(centers);


	//resolution = cubes' size in the collision world viewer
	CollisionWorldViewer CWV(resolution);
	

	//Calculating the average x, y and z coordinates to center the camera on the object
	minY=-centers.at(0).y;
	maxY=minY;
	minX=centers.at(0).x;
	maxX= minX;
	int j = 0;
	for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centers.begin() ; it != centers.end(); ++it){
		if(minY > -it->y)
			minY = -it->y;

		if(maxY < -it->y)
			maxY = -it->y;

		if(minX > it->x)
			minX = it->x;
		
		if(maxX < it->x)
			maxX = it->x;
		
		avg.setX( avg.getValue(Point::X) + it->x);
		avg.setY( avg.getValue(Point::Y) + it->y);
		avg.setZ( avg.getValue(Point::Z) + it->z);
		
		j++;
	}
	
	avg.setX( avg.getValue(Point::X) / j);
	avg.setY( avg.getValue(Point::Y) / j);
	avg.setZ( avg.getValue(Point::Z) / j);

	
	CWV.initPhysics(avg);
	CWV.addGround(minY - CWV.cubeSize);

	
	//Voxels creation cycle
	for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centers.begin() ; it != centers.end(); ++it){

		p = Point(it->x, it->y, it->z);

		CWV.addDefaultSizedRigidBody(p);
	
	}

	p = Point(objects.at(no).centroidX, objects.at(no).centroidY, objects.at(no).centroidZ);
	
	CWV.addSphere(p, resolution);
	/*CWV.addSphere(maxX*SCALING, objects.at(no).centroidY*SCALING, objects.at(no).centroidZ*SCALING, resolution*SCALING);
	CWV.addSphere(minX*SCALING, objects.at(no).centroidY*SCALING, objects.at(no).centroidZ*SCALING, resolution*SCALING);*/

	
	centroid = Point(objects.at(no).centroidX, objects.at(no).centroidY, objects.at(no).centroidZ);
	
	
	for(i = 0; i < cloud_path.size(); i++){

		if(i != no){

			//.pcd file loading
			if (!loadCloud(cloud_path.at(i)))
			return;

			octree.getOccupiedVoxelCenters(centers);

			minY=-centers.at(0).y;
			minX=centers.at(0).x;
			maxX= minX;
			
			for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centers.begin() ; it != centers.end(); ++it){
				if(minY > -it->y)
					minY = -it->y;

				if(minX > it->x)
					minX = it->x;
		
				if(maxX < it->x)
					maxX = it->x;
			}

			//Voxels creation cycle
			for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centers.begin() ; it != centers.end(); ++it){

				p = Point(it->x, it->y, it->z);

				CWV.addDefaultSizedRigidBody(p);
	
			}	
			p = Point(objects.at(no).centroidX, objects.at(no).centroidY, objects.at(no).centroidZ);
	
			CWV.addSphere(p, resolution);
			/*CWV.addSphere(maxX*SCALING, objects.at(no).centroidY*SCALING, objects.at(no).centroidZ*SCALING, resolution*SCALING);
			/*CWV.addSphere(minX*SCALING, objects.at(i).centroidY*SCALING, objects.at(i).centroidZ*SCALING, resolution*SCALING);
			CWV.addSphere(maxX*SCALING, objects.at(i).centroidY*SCALING, objects.at(i).centroidZ*SCALING, resolution*SCALING);*/


		}

	}
	

	
	vector<Point> myPath;
	centroid.cwToStandard();
	std::cout << "CENTROID(" << centroid.getValue(Point::X) << ", " << centroid.getValue(Point::Y) <<", " << centroid.getValue(Point::Z) << ")" << std::endl;
	invKinResult ikr = invKin(centroid, true);
	centroid.standardToCw();
	
	if(ikr.reachable){	
		start = Point(STARTX, STARTY, STARTZ);
		switch(movementClass){

			case 0:
				myPath.push_back(Point(STARTX, STARTZ, -STARTY));
				frontalApproach(start, centroid, &CWV, &myPath);
				break;
				
			case 1:
				myPath.push_back(Point(STARTX, STARTZ, -STARTY));
				upperApproach(start, centroid, &CWV, &myPath, maxY);
				break;

			default:
				std::cout << "EMG signal error" << std::endl;
				break;
		}
	}
	else
		std::cout << "OBJECT UNREACHABLE" << std::endl;

	std::cout << "\nNumber of steps: " << myPath.size() << std::endl;
	this->setPath(myPath);
	


	#ifdef CHECK_MEMORY_LEAKS
		ccdDemo.exitPhysics();
	#else
		if(SHOW_GRAPHICS){

			char **exeName;
			exeName[0] = "EMG.exe\0";
	
			glutmain(0, exeName,1024,600,"Collision world",&CWV);
			
		}
	#endif



}

 PathCalculator::PathCalculator (int movementClass) :
    cloud (new pcl::PointCloud<pcl::PointXYZ>()), octree (1)
   {

	Point start;
	vector<Point> myPath;
	//centroid.standardToCw();
	dirKinResult dkr;

	start = Point(STARTX, STARTY, STARTZ);
	switch(movementClass){

		case 0:
			myPath.push_back(Point(STARTX, STARTZ, -STARTY));
			dkr = dirKin(0,70,0,0);
			myPath.push_back(dkr.handCenter);
			dkr = dirKin(89,0,0,0);
			myPath.push_back(dkr.handCenter);
			break;

		case 1:
			myPath.push_back(Point(STARTX, STARTZ, -STARTY));
			dkr = dirKin(0,90,0,0);
			myPath.push_back(dkr.handCenter);
			break;

		default:
			std::cout << "EMG signal error" << std::endl;
			break;

	}
	
	std::cout << "\nNumber of steps: " << myPath.size() << std::endl;
	this->setPath(myPath);
	

  }

  /* \brief Helper function that read a pointcloud file (returns false if pbl)
   *  Also initialize the octree
   *
   */
  bool PathCalculator::loadCloud(std::string &filename)
  {

	pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> *octreeAux = new pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>(0.005);
	
    //read cloud
    if (pcl::io::loadPCDFile(filename, *cloud))
    {
      std::cerr << "ERROR: Cannot open file " << filename << "! Aborting..." << std::endl;
      return false;
    }

    //remove NaN Points
    std::vector<int> nanIndexes;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, nanIndexes);
    //std::cout << "Loaded " << cloud->points.size() << " points" << std::endl;

	
    //create octree structure
    octreeAux->setInputCloud(cloud);
    //update bounding box automatically
    octreeAux->defineBoundingBox();
    //add points in the tree
    octreeAux->addPointsFromInputCloud();

	octree = *octreeAux;

    return true;
  }

  std::vector<Point> PathCalculator::getPath(){
	  return this->path;
  }

  void PathCalculator::setPath(vector<Point> myPath){
	  this->path = myPath;
  }

 bool move(Point start, Point object, CollisionWorldViewer* CWV, vector<Point> *path, bool checkCollisions){


	std::cout <<	"\n\n>-------------------------------------<" << std::endl;

	invKinResult ikr1, ikr2;
	dirKinResult dkr;
	float incTeta1, incTeta2, incTeta3;
	double xc, yc, zc;
	int flag;
	Point currentPoint;

	/*float a,b,c;
	printf("teta1: ");
	scanf("%f",&a);
	printf("teta2: ");
	scanf("%f",&b);
	printf("teta3: ");
	scanf("%f",&c); 

	dkr = dirKin(a,b,c,0);
	std::cout << "\n\nx: " << dkr.handCenter.x << "\ny: " << dkr.handCenter.y << "\nz: " << dkr.handCenter.z << std::endl;
	*/

	//ikr = invKin(dkr.handCenter.x, dkr.handCenter.y, dkr.handCenter.z);


	//Incremental value calculation
	object.cwToStandard();
	start.cwToStandard();
	ikr1 = invKin(object);
	ikr2 = invKin(start);
	
	if(!ikr2.reachable){
		std::cout << "Starting point (" << start.getValue(Point::X) << ", " << start.getValue(Point::Y) <<", " << start.getValue(Point::Z) << ") unreachable" << std::endl;
		return 0;
	}

	if(!ikr1.reachable){
		std::cout << "Target point (" << object.getValue(Point::X) << ", " << object.getValue(Point::Y) <<", " << object.getValue(Point::Z) << ") unreachable" << std::endl;
		return 0;
	}

	incTeta1 = (ikr1.teta1 - ikr2.teta1) / NUMBER_OF_TRAJECTORY_POINTS;
	incTeta2 = (ikr1.teta2 - ikr2.teta2) / NUMBER_OF_TRAJECTORY_POINTS;
	incTeta3 = (ikr1.teta3 - ikr2.teta3) / NUMBER_OF_TRAJECTORY_POINTS;

	std::cout <<	"\n\nMoving from:\n\n Start:\n  x: " << start.getValue(Point::X) << "\n  y: " << start.getValue(Point::Z) << "\n  z: " << -start.getValue(Point::Y)
			  <<	"\n\nTo:\n End:\n  x: " << object.getValue(Point::X) << "\n  y: " << object.getValue(Point::Z) << "\n  z: " << -object.getValue(Point::Y) 
			  <<	"\n\nTetaStart:(" << ikr2.teta1 << ", " << ikr2.teta2 << ", " << ikr2.teta3 << ")"
			  <<	"\nTetaEnd:( " << ikr1.teta1 << ", " << ikr1.teta2 << ", " << ikr1.teta3 << ")" << std::endl;




	/*std::cout << "\n\nx: " << object.getValue(Point::X) << "\ny: " << object.getValue(Point::Z) << "\nz: " << -object.getValue(Point::Y) << std::endl;
	std::cout << "\n\nx: " << start.getValue(Point::X) << "\ny: " << start.getValue(Point::Z) << "\nz: " << -start.getValue(Point::Y) << std::endl;
	std::cout << "\n\nteta1: " << ikr1.teta1 << "\nteta2: " << ikr1.teta2 << "\nteta3: " << ikr1.teta3 << std::endl;
	std::cout << "\n\nteta1: " << ikr2.teta1 << "\nteta2: " << ikr2.teta2 << "\nteta3: " << ikr2.teta3 << std::endl;
	std::cout << "\nincTeta1: " << incTeta1 << "\nincteta2: " << incTeta2 << "\nincteta3: " << incTeta3 << std::endl;*/
	
	object.standardToCw();
	start.standardToCw();

	flag = 1;
	//Collision checking loop
	int i;
	for(i = 0; i <= 20; i++){
		
		dkr = dirKin(ikr2.teta1 + incTeta1*i, ikr2.teta2 + incTeta2*i, ikr2.teta3 + incTeta3*i, 0);  
	
		xc = (dkr.handCenter.getValue(Point::X) - dkr.wrist.getValue(Point::X)) / HAND_CENTER_OFFSET;
		yc = (dkr.handCenter.getValue(Point::Y) - dkr.wrist.getValue(Point::Y)) / HAND_CENTER_OFFSET;
		zc = (dkr.handCenter.getValue(Point::Z) - dkr.wrist.getValue(Point::Z)) / HAND_CENTER_OFFSET;
	

		flag = CWV->addHand(		xc,yc,zc,
									dkr.graspingPoint1,
									dkr.graspingPoint2, 
									FINGER_SIZE*SCALING, FINGER_LENGTH*SCALING, GRASP_WIDTH*SCALING, HAND_HEIGHT*SCALING, HAND_LENGTH*SCALING,
									(ikr2.teta1 + incTeta1*i)*M_PI/180, (ikr2.teta2 + incTeta2*i)*M_PI/180, (ikr2.teta3 + incTeta3*i)*M_PI/180);


		if(flag == 0 && checkCollisions){
			std::cout << "\nCOLLISION DETECTED: " << flag << std::endl;
			std::cout << "\nSTEP: " << i << std::endl;

			std::cout <<	"\n\nPosition:"
						"\n   X:\t" << object.getValue(Point::X) <<
						"\n   Y:\t" << object.getValue(Point::Y) <<
						"\n   Z:\t" << -object.getValue(Point::Z) << std::endl;

		}
		
		//std::cin >> flag;
		
		currentPoint = dkr.handCenter;
		 
		std::cout << "\nCurrent point: X: " << currentPoint.getValue(Point::X) << " - Y: " << currentPoint.getValue(Point::Y) << " - Z: " << currentPoint.getValue(Point::Z);

		/*if(i > 1){
			
			if( (path->at( path->size()-1).distance(currentPoint) <= (path->at( path->size()-1).distance(path->at( path->size()-2)))*3.2) ){
				std::cout << "\nOK: " << path->at( path->size()-1).distance(currentPoint) << " < " << (path->at( path->size()-1).distance(path->at( path->size()-2)))*3.2;
				path->push_back(currentPoint);
			}
			else
				std::cout << "\nERROR: " << path->at( path->size()-1).distance(currentPoint) << " > " << (path->at( path->size()-1).distance(path->at( path->size()-2)))*3.2 << std::endl;
		}
		else{
			path->push_back(currentPoint);
		}*/

		if(i==20)
			path->push_back(currentPoint);

		if((flag == 1 || (!checkCollisions)) && i != 20)
			CWV->removeHand();
		else
			break;

	}


	return (flag || (!checkCollisions)); 

}

bool lmove(Point start, Point object, CollisionWorldViewer* CWV, vector<Point> *path){

	bool flag;
	dirKinResult dkr;
	invKinResult ikr;

	dkr = dirKin(0,70,0,0);
	dkr.handCenter.standardToCw();
	Point p1 = dkr.handCenter;

	flag = move(start, p1, CWV, path, true);
	if(flag == 0)
		return flag;
	
	object.cwToStandard();
	ikr = invKin(object);

	
	dkr = dirKin(0, 70, ikr.teta3, 0);
	


	dkr.handCenter.standardToCw();
	Point p2 = dkr.handCenter;

	CWV->removeHand();
	flag = move(p1, p2, CWV, path, true);
	if(flag == 0)
		return flag;

	object.standardToCw();
	CWV->removeHand();
	flag = move(p2, object, CWV, path, true);


	return flag;
}


void frontalApproach(Point start, Point object, CollisionWorldViewer* CWV, vector<Point> *path){

	bool flag;
	Point p = object;
	p.setZ( p.getValue(Point::Z) - HAND_CENTER_OFFSET*4 );
	
	/*std::cout << "\n\nTrying frontal approach, direct move to (" << object.getValue(Point::X) << ", " << object.getValue(Point::Y) << ", " << object.getValue(Point::Z) << ")" << std::endl; 
	flag = move(start, p, CWV, path, true);
	if(flag == 1){

		CWV->removeHand();
		flag = move(p, object, CWV, path, false);
		if(flag == 1)
			return ;
	}*/

	
	std::cout << "\n\nTrying frontal approach, lateral move to (" << object.getValue(Point::X) << ", " << object.getValue(Point::Y) << ", " << object.getValue(Point::Z) << ")" << std::endl;
	//path->clear();
	//path->push_back(Point(STARTX, STARTZ, -STARTY));
	//CWV->removeHand();
	

	flag = lmove(start, p, CWV, path);
	if(flag == 0)
		return ;


	CWV->removeHand();
	flag = move(p, object, CWV, path, true);

}

void upperApproach(Point start, Point object, CollisionWorldViewer* CWV, vector<Point> *path, double maxY){

	
	bool flag;
	Point p = object;
	p.setY( maxY - 8*HAND_CENTER_OFFSET );

	std::cout << "Pre-approach point (" << p.getValue(Point::X) << ", " << p.getValue(Point::Y) << ", " << p.getValue(Point::Z) << ")" << std::endl;
	
	std::cout << "\n\nTrying upper approach, lateral move to (" << object.getValue(Point::X) << ", " << object.getValue(Point::Y) << ", " << object.getValue(Point::Z) << ")" << std::endl;
	//path->clear();
	//path->push_back(Point(STARTX, STARTZ, -STARTY));
	//CWV->removeHand();
	

	flag = lmove(start, p, CWV, path);
	if(flag == 0)
		return ;


	CWV->removeHand();
	flag = move(p, object, CWV, path, true);

}